/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <osl_core/ModuleCore.h>
#include <osl_core/ConfigWrapper.h>
   #include <unistd.h>

namespace osl_core
{

// ---------------------------------------------------------------------------
ConfigWrapper::ConfigWrapper()
{
	initCalled_ = false;
	usingINIFile_ = false;
}


// ---------------------------------------------------------------------------
bool ConfigWrapper::initROSParam(ros::NodeHandlePtr handle, const std::string &basename)
{
	if (initCalled_) {
		PRINTLN_ERR("ConfigWrapper::init cannot be called more than once.");
		return false;
	}

	initCalled_ = true;

	rosHandle_ = handle;
	rosParamBase_ = basename;

	// Add trailing '/' to basename if it doesn't have one.
	if (rosParamBase_.length() > 0 && rosParamBase_.at(rosParamBase_.length()-1) != '/') {
		rosParamBase_ += '/';
	}

	return true;
}

// ---------------------------------------------------------------------------
std::string ConfigWrapper::olibINIParamToROSParam(const std::string &name, const std::string &section)
{
	// For ROS, we want:
	// paramName = "/<base>/<section>/<name>"
	// But we want section to be converted to lower case first.

	// Add '/' and convert section to lower case.
	std::string paramName = rosParamBase_ + section;
	std::transform(paramName.begin(), paramName.end(), paramName.begin(), ::tolower);
	// Add '/' and name.
	paramName += '/' + name;

	return paramName;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getInt(int &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParam(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getFloat(float &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	double varDouble;
	if (!rosHandle_->getParam(paramName, varDouble)) {
		printNoParam(paramName, section);
		return false;
	}
	var = (float)varDouble;
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getDouble(double &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParam(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getString(std::string &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParam(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getBool(bool &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParam(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}
// ---------------------------------------------------------------------------
bool ConfigWrapper::getIntCached(int &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParamCached(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getFloatCached(float &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	double varDouble;
	if (!rosHandle_->getParamCached(paramName, varDouble)) {
		printNoParam(paramName, section);
		return false;
	}
	var = (float)varDouble;
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getDoubleCached(double &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParamCached(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getStringCached(std::string &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParamCached(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::getBoolCached(bool &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	if (!rosHandle_->getParamCached(paramName, var)) {
		printNoParam(paramName, section);
		return false;
	}
	return true;
}
// ---------------------------------------------------------------------------
void ConfigWrapper::setInt(int var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	rosHandle_->setParam(paramName, var);
}

// ---------------------------------------------------------------------------
void ConfigWrapper::setFloat(float var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	rosHandle_->setParam(paramName, var);
}

// ---------------------------------------------------------------------------
void ConfigWrapper::setDouble(double var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	rosHandle_->setParam(paramName, var);
}

// ---------------------------------------------------------------------------
void ConfigWrapper::setString(const std::string &var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	rosHandle_->setParam(paramName, var);
}

// ---------------------------------------------------------------------------
void ConfigWrapper::setBool(bool var, const std::string &name, const std::string &section)
{
	std::string paramName = olibINIParamToROSParam(name, section);
	rosHandle_->setParam(paramName, var);
}

// ---------------------------------------------------------------------------
void ConfigWrapper::printNoParam(const std::string &name, const std::string &section)
{
	// section is ignored here, but kept so the method prototype is the same as for INI file version.
	PRINTLN_WARN("No ROS parameter " << name);
}

// ---------------------------------------------------------------------------
bool ConfigWrapper::save()
{
	// ROS parameters aren't saved in this way - they persist in the master node.
	return true;
}

// ---------------------------------------------------------------------------
ConfigWrapper::~ConfigWrapper()
{
	// TODO cleanup
}



}
